#!/bin/bash

# 定义路径变量
home_dir="/home/nvidia"
ros_bash="/opt/ros/foxy/setup.bash"
ros_ws_dir="${home_dir}/ros2_ws"
dds2ros_dir="${ros_ws_dir}/install/dds2ros"
ros_home="${home_dir}/.ros" #  ros工作目录
log_dir="${ros_home}/log/latest" # 节点日志文件目录 
bag_dir="${ros_home}/bags" # bag文件保存目录
max_freq_script="${home_dir}/droneyee_fly/max_cpu_freq.sh"  # 执行调整CPU频率的脚本
gpu_freq_script="${home_dir}/droneyee_fly/max_gpu_freq.sh"  # 执行调整GPU频率的脚本

basepath=$(cd "$(dirname "$0")" || exit; pwd)  # 获取脚本所在目录的绝对路径
time=$(date +%Y-%m-%d_%H:%M:%S)  # 获取当前时间，用于日志文件命名

# 清空指定目录
echo nvidia | sudo -S rm -rf "${log_dir}/"*
echo nvidia | sudo -S rm -rf "${bag_dir}/"*
# 清空回收站
echo nvidia | sudo -S rm -rf ~/.local/share/Trash/*
mkdir -p "$log_dir"  # 确保日志目录存在
mkdir -p "$bag_dir"  # 确保bag文件保存目录存在

# 启动一个ROS2节点的函数，使用gnome-terminal打开一个新的标签
launch_ros2_node() {
    local node_name=$1
    local command=$2 #启动命令
    local log_file="${log_dir}/${node_name}_${time}.log"  # 节点日志文件路径
    local full_command
    
    # 根据参数判断是否保存日志文件，构建相应的命令
    if [[ $3 == true ]]; then
        # 如果传入true，则同时保存到日志文件
        full_command="${command} |& tee -a ${log_file}"
    else
        # 否则只执行命令
        full_command="${command}"
    fi
    
    # 记录节点启动信息到日志文件
    echo "$(date +%Y-%m-%d) $(date +%H:%M:%S) ${node_name}" >>"${log_file}"
    # 在gnome-terminal中打开新标签并运行ROS2节点
    gnome-terminal --tab -t "${node_name}" -- bash -c "source ${ros_bash}; source ${ros_ws_dir}/install/setup.bash; export ROS_HOME=${ros_home}; cd ${ros_ws_dir}; ${full_command};"
        
}	
																

# 设置命令，如调整CPU和GPU频率
echo nvidia | sudo -S sh "${max_freq_script}"  # 执行调整CPU频率的脚本
sleep 1
echo nvidia | sudo -S sh "${gpu_freq_script}"  # 执行调整GPU频率的脚本
sleep 1
echo nvidia | sudo -S chmod 777 /dev/ttyTHS0  # 设置设备权限
echo nvidia | sudo -S chmod 777 /dev/ttyUSB0  # 设置设备权限


gnome-terminal -- bash -c "source ${ros_bash}; source ${ros_ws_dir}/install/setup.bash; export ROS_HOME=${ros_home};"

# 启动节点之前的延时
sleep 1


# 启动各个节点
# 启动mavros 节点
gnome-terminal --tab -t "1_mavros" -- bash -c "ros2 run mavros mavros_node --ros-args --params-file /home/nvidia/ros2_ws/mavros_param_1.yaml;"  
sleep 7
launch_ros2_node "2_fcu_comm" "ros2 launch fcu_comm fcu_comm.launch" true # 启动底层控制节点
sleep 7
launch_ros2_node "3_Cluster_formation_control" "ros2 run new_cluster Cluster_formation_control_node_new" true  # 启动编队节点
sleep 1
launch_ros2_node "4_DDS2ROS" "cd ${dds2ros_dir}; ./DDS2ROS" true  # 启动 DDS2ROS 节点
sleep 1
launch_ros2_node "5_behavior_node" "ros2 run behavior-process behavior-node" true # 启动行为树节点
sleep 5
launch_ros2_node "6_video_distribution" "ros2 launch video_distribution video_distribution.launch.py;" true # 启动视频分发节点
sleep 1
launch_ros2_node "7_yolov5" "ros2 run trace_topic yolo_sort_object_pub" true # 启动yolo图像识别节点
sleep 1
launch_ros2_node "8_vision_locate" "ros2 launch fcu_comm vision_locate.launch" true # 启动位置解算节点
sleep 1
launch_ros2_node "9_q10f" "ros2 launch q10f_pkg q10f.launch" true  # 启动吊舱节点
sleep 1
:'
launch_ros2_node "9_multispectral_camera" "ros2 launch multispectral_camera multispectral_camera.launch.xml" true  # 启动多光谱控制节点
sleep 1
launch_ros2_node "10_rtp2img" "ros2 launch rtp2img rtp2img.launch.py" true  # 启动多光谱rtp转ros图片节点
sleep 1
launch_ros2_node "7_xe_thru" "ros2 launch xe_thru x4m300.launch.xml" true  # 启动生命探测雷达节点
sleep 1
launch_ros2_node "8_usb_cam_pkg" "ros2 launch usb_cam_pkg mt2303u.launch" true  # 启动usb相机节点
'
# 启动检查脚本
echo "$(date +%Y-%m-%d) $(date +%H:%M:%S) 启动所有节点并开始检查" >>"${log_dir}/startup_${time}.log"
gnome-terminal -- bash -c "source ${ros_bash}; source ${ros_ws_dir}/install/setup.bash; export ROS_HOME=${ros_home}; bash ${basepath}/check_restart.sh;"

'
# 启动bag记录
topics=("/inner/topic1" "/outer/topic2" "/another/topic3")  # 替换为实际要记录的话题列表
ros2_bag_pid=""

start_bag_record() {
    gnome-terminal --tab -t "ros2_bag_record" -- bash -c "source ${ros_bash}; ros2 bag record -o ${bag_dir}/all_nodes_${time}.bag ${topics[*]}"
    ros2_bag_pid=$!
}

stop_bag_record() {
    if [ -n "$ros2_bag_pid" ]; then
        kill -SIGINT "$ros2_bag_pid"
        wait "$ros2_bag_pid"
        ros2_bag_pid=""
    fi
}

start_bag_record

trap stop_bag_record SIGINT SIGTERM

wait
